// This file is part of the AliceVision project.
// Copyright (c) 2016 AliceVision contributors.
// Copyright (c) 2012 openMVG contributors.
// This Source Code Form is subject to the terms of the Mozilla Public License,
// v. 2.0. If a copy of the MPL was not distributed with this file,
// You can obtain one at https://mozilla.org/MPL/2.0/.

#include "l1.hpp"
#include <aliceVision/system/Logger.hpp>

#ifdef ALICEVISION_ROTATION_AVERAGING_WITH_BOOST
    #include <boost/graph/adjacency_list.hpp>
    #include <boost/graph/kruskal_min_spanning_tree.hpp>
    #include <boost/graph/prim_minimum_spanning_tree.hpp>
    #include <boost/accumulators/accumulators.hpp>
    #include <boost/accumulators/statistics/stats.hpp>
    #include <boost/accumulators/statistics/min.hpp>
    #include <boost/accumulators/statistics/mean.hpp>
    #include <boost/accumulators/statistics/max.hpp>
    #include <boost/foreach.hpp>
#else
    #include "lemon/list_graph.h"
    #include "lemon/kruskal.h"
    #include "lemon/adaptors.h"
    #include "lemon/dfs.h"
    #include "lemon/path.h"
#endif

#include "ceres/ceres.h"
#include "ceres/rotation.h"

#include <aliceVision/numeric/BoxStats.hpp>

#include <map>
#include <queue>
#include <stdint.h>

#ifdef ALICEVISION_ROTATION_AVERAGING_WITH_BOOST
using namespace boost;
#else
using namespace lemon;
#endif

namespace aliceVision {
namespace rotationAveraging {
namespace l1 {

// Minimum l1 error approximation:
//
// Let A be a M x N matrix with full rank. Given y of R^M, the problem
// (PA) minx ||y - Ax||1
// finds the vector x of R^N such that the error y - Ax has minimum l1 norm
// (i.e. we are asking that the difference between Ax and y be sparse).
// This problem arises in the context of channel coding
// (see E. J. Candes and T. Tao. "Decoding by linear programming". in IEEE Trans.
// Inform. Theory, December 2005).
//
// Suppose we have a channel code that produces a codeword c = Ax for a message x. The
// message travels over the channel, and has an unknown number of its entries corrupted.
// The decoder observes y = c + e, where e is the corruption. If e is sparse enough, then
// the decoder can use (PA) to recover x exactly. When x, A, y have real-valued entries,
// (PA) can be recast as an LP.
//
template<typename MATRIX_TYPE>
inline bool TRobustRegressionL1PD(const MATRIX_TYPE& A,
                                  const Eigen::Matrix<REAL, Eigen::Dynamic, 1>& y,
                                  Eigen::Matrix<REAL, Eigen::Dynamic, 1>& xp,
                                  REAL pdtol,
                                  unsigned pdmaxiter)
{
    typedef Eigen::Matrix<REAL, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> Matrix;
    typedef Eigen::Matrix<REAL, Eigen::Dynamic, 1> Vector;
    const unsigned M = (unsigned)y.size();
    const unsigned N = (unsigned)xp.size();
    assert(A.rows() == M && A.cols() == N);

    const REAL alpha(0.01);
    const REAL beta(0.5);
    const REAL mu(10);

    Vector x(xp);
    Vector Ax(A * x);
    Vector tmpM1(y - Ax);
    Vector tmpM2(-tmpM1);
    Vector tmpM3(tmpM1.cwiseAbs()), tmpM4(M);
    Vector u = (tmpM3 * REAL(0.95)).array() + tmpM3.maxCoeff() * REAL(0.10);
    Vector fu1 = tmpM2 - u;
    Vector fu2 = tmpM1 - u;

    Vector lamu1(M), lamu2(M);
    for (unsigned i = 0; i < M; ++i)
    {
        lamu1(i) = -1.0 / fu1(i);
        lamu2(i) = -1.0 / fu2(i);
    }
    const MATRIX_TYPE At(A.transpose());
    Vector Atv(At * (lamu1 - lamu2));
    REAL AtvNormSq = Atv.squaredNorm();
    Vector rdual((-lamu1 - lamu2).array() + REAL(1));
    REAL rdualNormSq = rdual.squaredNorm();

    Vector w2(M), sig1(M), sig2(M), sigx(M), dx(N), up(N), Atdv(N);
    Vector Axp(M), Atvp(M);
    Vector &Adx(sigx), &du(w2), &w1p(dx);
    Matrix H11p(N, N);
    Vector &dlamu1(tmpM3), &dlamu2(tmpM4);
    for (unsigned pditer = 0; pditer < pdmaxiter; ++pditer)
    {
        // surrogate duality gap
        const REAL sdg(-(fu1.dot(lamu1) + fu2.dot(lamu2)));
        if (sdg < pdtol)
            break;
        const REAL tau(mu * 2 * M / sdg);
        const REAL inv_tau = REAL(-1) / tau;
        tmpM1 = (-lamu1.cwiseProduct(fu1)).array() + inv_tau;
        tmpM2 = (-lamu2.cwiseProduct(fu2)).array() + inv_tau;
        const REAL resnorm = sqrt(AtvNormSq + rdualNormSq + tmpM1.squaredNorm() + tmpM2.squaredNorm());

        for (unsigned i = 0; i < M; ++i)
        {
            REAL& tmpM3i = tmpM3(i);
            tmpM3i = inv_tau / fu1(i);
            REAL& tmpM4i = tmpM4(i);
            tmpM4i = inv_tau / fu2(i);
            w2(i) = tmpM3i + tmpM4i - REAL(1);
        }

        tmpM1 = lamu1.cwiseQuotient(fu1);
        tmpM2 = lamu2.cwiseQuotient(fu2);
        sig1 = -tmpM1 - tmpM2;
        sig2 = tmpM1 - tmpM2;
        sigx = sig1 - sig2.cwiseAbs2().cwiseQuotient(sig1);

        H11p = At * (Eigen::DiagonalMatrix<REAL, Eigen::Dynamic>(sigx) * A);
        w1p = At * (tmpM4 - tmpM3 - (sig2.cwiseQuotient(sig1).cwiseProduct(w2)));

        // optimized solver as A is positive definite and symmetric
        dx = H11p.ldlt().solve(w1p);

        Adx = A * dx;

        du = (w2 - sig2.cwiseProduct(Adx)).cwiseQuotient(sig1);

        dlamu1 = -tmpM1.cwiseProduct(Adx - du) - lamu1 + tmpM3;
        dlamu2 = tmpM2.cwiseProduct(Adx + du) - lamu2 + tmpM4;
        Atdv = At * (dlamu1 - dlamu2);

        // make sure that the step is feasible: keeps lamu1,lamu2 > 0, fu1,fu2 < 0
        REAL s(1);
        for (unsigned i = 0; i < M; ++i)
        {
            REAL& dlamu1i = dlamu1(i);
            if (dlamu1i < 0)
            {
                const REAL tmp = -lamu1(i) / dlamu1i;
                if (s > tmp)
                    s = tmp;
            }
            REAL& dlamu2i = dlamu2(i);
            if (dlamu2i < 0)
            {
                const REAL tmp = -lamu2(i) / dlamu2i;
                if (s > tmp)
                    s = tmp;
            }
        }
        for (unsigned i = 0; i < M; ++i)
        {
            REAL& Adxi = Adx(i);
            REAL& dui = du(i);
            const REAL Adx_du = Adxi - dui;
            if (Adx_du > 0)
            {
                const REAL tmp = -fu1(i) / Adx_du;
                if (s > tmp)
                    s = tmp;
            }
            const REAL _Adx_du = -Adxi - dui;
            if (_Adx_du > 0)
            {
                const REAL tmp = -fu2(i) / _Adx_du;
                if (s > tmp)
                    s = tmp;
            }
        }
        s *= REAL(0.99);

        // backtrack
        lamu1 += s * dlamu1;
        lamu2 += s * dlamu2;
        rdual = (-lamu1 - lamu2).array() + REAL(1);
        rdualNormSq = rdual.squaredNorm();
        bool suffdec = false;
        unsigned backiter = 0;
        do
        {
            xp = x + s * dx;
            up = u + s * du;
            Axp = Ax + s * Adx;
            Atvp = Atv + s * Atdv;
            fu1 = Axp - y - up;
            fu2 = -Axp + y - up;
            AtvNormSq = Atvp.squaredNorm();
            tmpM1 = (-lamu1.cwiseProduct(fu1)).array() + inv_tau;
            tmpM2 = (-lamu2.cwiseProduct(fu2)).array() + inv_tau;
            const REAL newresnorm = sqrt(AtvNormSq + rdualNormSq + tmpM1.squaredNorm() + tmpM2.squaredNorm());
            suffdec = (newresnorm <= (REAL(1) - alpha * s) * resnorm);
            s = beta * s;
            if (++backiter > 32)
            {
                //("error: stuck backtracking, returning last iterate"); // see Section 4 of notes for more information
                xp.swap(x);
                return false;
            }
        } while (!suffdec);

        // next iteration
        x.swap(xp);
        u.swap(up);
        Ax.swap(Axp);
        Atv.swap(Atvp);
    }
    return true;
}

bool RobustRegressionL1PD(const Eigen::Matrix<REAL, Eigen::Dynamic, Eigen::Dynamic>& A,
                          const Eigen::Matrix<REAL, Eigen::Dynamic, 1>& b,
                          Eigen::Matrix<REAL, Eigen::Dynamic, 1>& x,
                          REAL pdtol,
                          unsigned pdmaxiter)
{
    return TRobustRegressionL1PD(A, b, x, pdtol, pdmaxiter);
}
bool RobustRegressionL1PD(const Eigen::SparseMatrix<REAL, Eigen::ColMajor>& A,
                          const Eigen::Matrix<REAL, Eigen::Dynamic, 1>& b,
                          Eigen::Matrix<REAL, Eigen::Dynamic, 1>& x,
                          REAL pdtol,
                          unsigned pdmaxiter)
{
    return TRobustRegressionL1PD(A, b, x, pdtol, pdmaxiter);
}

/*----------------------------------------------------------------*/

// Iteratively Re-weighted Least Squares (IRLS) implementation
template<typename MATRIX_TYPE>
inline bool TIterativelyReweightedLeastSquares(const MATRIX_TYPE& A,
                                               const Eigen::Matrix<REAL, Eigen::Dynamic, 1>& b,
                                               Eigen::Matrix<REAL, Eigen::Dynamic, 1>& x,
                                               REAL sigma,
                                               REAL eps)
{
    typedef Eigen::Matrix<REAL, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> Matrix;
    typedef Eigen::Matrix<REAL, Eigen::Dynamic, 1> Vector;
    const unsigned m = (unsigned)b.size();
    const unsigned n = (unsigned)x.size();
    assert(A.rows() == m && A.cols() == n);

    // iterate optimization till the desired precision is reached
    Vector xp(n), e(m);
    const REAL sigmaSq(Square(sigma));
    unsigned iter = 0;
    REAL delta = std::numeric_limits<REAL>::max(), deltap;
    do
    {
        xp = x;
        // compute error vector
        e = A * x - b;
        // compute robust errors using the Huber-like loss function
        for (unsigned i = 0; i < m; ++i)
        {
            REAL& err = e(i);
            const REAL errSq(Square(err));
            err = sigmaSq / (errSq + sigmaSq);
        }
        // solve the linear system using l2 norm
        const MATRIX_TYPE AtF(A.transpose() * e.asDiagonal());
        const Eigen::LDLT<Matrix> solver(AtF * A);  // compute the Cholesky decomposition
        if (solver.info() != Eigen::Success)
        {
            ALICEVISION_LOG_WARNING("error: decomposing linear system failed");
            return false;
        }
        x = solver.solve(AtF * b);
        if (solver.info() != Eigen::Success)
        {
            ALICEVISION_LOG_WARNING("error: solving linear system failed");
            return false;
        }
        if (++iter > 32)
            break;
        deltap = delta;
        delta = (xp - x).norm();
    } while (delta > eps && (deltap - delta) / delta > 1e-2);
    return true;
}

bool IterativelyReweightedLeastSquares(const Eigen::Matrix<REAL, Eigen::Dynamic, Eigen::Dynamic>& A,
                                       const Eigen::Matrix<REAL, Eigen::Dynamic, 1>& b,
                                       Eigen::Matrix<REAL, Eigen::Dynamic, 1>& x,
                                       REAL sigma,
                                       REAL eps)
{
    return TIterativelyReweightedLeastSquares(A, b, x, sigma, eps);
}
bool IterativelyReweightedLeastSquares(const Eigen::SparseMatrix<REAL, Eigen::ColMajor>& A,
                                       const Eigen::Matrix<REAL, Eigen::Dynamic, 1>& b,
                                       Eigen::Matrix<REAL, Eigen::Dynamic, 1>& x,
                                       REAL sigma,
                                       REAL eps)
{
    return TIterativelyReweightedLeastSquares(A, b, x, sigma, eps);
}

/////////////////////////

// given an array of values, compute the X84 threshold as in:
// Hampel FR, Rousseeuw PJ, Ronchetti EM, Stahel WA
// "Robust Statistics: the Approach Based on Influence Functions"
// Wiley Series in Probability and Mathematical Statistics, John Wiley & Sons, 1986
// returns the pair(median,trust_region)
// upper-bound threshold = median+trust_region
// lower-bound threshold = median-trust_region
template<typename TYPE>
inline std::pair<TYPE, TYPE> ComputeX84Threshold(const TYPE* const values, size_t size, TYPE mul = TYPE(5.2))
{
    assert(size > 0);
    typename std::vector<TYPE> data(values, values + size);
    typename std::vector<TYPE>::iterator mid = data.begin() + size / 2;
    std::nth_element(data.begin(), mid, data.end());
    const TYPE median = *mid;
    // threshold = 5.2 * MEDIAN(ABS(values-median));
    for (size_t i = 0; i < size; ++i)
        data[i] = std::abs(values[i] - median);
    std::nth_element(data.begin(), mid, data.end());
    return std::make_pair(median, mul * (*mid));
}  // ComputeX84Threshold

/////////////////////////

typedef aliceVision::Mat3 Matrix3x3;
typedef std::vector<size_t> IndexArr;

// find the shortest cycle for the given graph and starting vertex
struct Node
{
    typedef IndexArr InternalType;
    InternalType edges;  // array of vertex indices
};
typedef std::vector<Node> NodeArr;

struct Link
{
    size_t ID;        // node index
    size_t parentID;  // parent link
    inline Link(size_t _ID = 0, size_t _parentID = 0)
      : ID(_ID),
        parentID(_parentID)
    {}
};
typedef std::queue<Link> LinkQue;

#ifdef ALICEVISION_ROTATION_AVERAGING_WITH_BOOST
typedef boost::property<boost::edge_weight_t, float> edge_property_t;
typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS, size_t, edge_property_t> graph_t;
typedef graph_t::vertex_descriptor vertex_t;
typedef graph_t::edge_descriptor edge_t;
typedef boost::graph_traits<graph_t>::edge_iterator edge_iter;
#else
typedef lemon::ListGraph graph_t;
typedef graph_t::EdgeMap<double> map_EdgeMap;
#endif
typedef std::map<std::pair<size_t, size_t>, Matrix3x3> MapEdgeIJ2R;

// Look for the maximum spanning tree along the graph of relative rotations
// since we look for the maximum spanning tree using a minimum spanning tree algorithm
// weight are negated.
size_t FindMaximumSpanningTree(const RelativeRotations& RelRs, graph_t& g, MapEdgeIJ2R& mapIJ2R, NodeArr& minGraph)
{
    assert(!RelRs.empty());
#ifdef ALICEVISION_ROTATION_AVERAGING_WITH_BOOST
    for (size_t p = 0; p < RelRs.size(); ++p)
    {
        const RelativeRotation& relR = RelRs[p];
        boost::add_edge(relR.i, relR.j, -relR.weight, g);
        mapIJ2R[std::make_pair(relR.i, relR.j)] = relR.Rij;
        mapIJ2R[std::make_pair(relR.j, relR.i)] = relR.Rij.transpose();
    }
    // find the minimum spanning tree
    const size_t nViews = boost::num_vertices(g);
    minGraph.resize(nViews);
    std::vector<edge_t> spanningTree;
    boost::kruskal_minimum_spanning_tree(g, std::back_inserter(spanningTree));
    for (std::vector<edge_t>::const_iterator ei = spanningTree.begin(); ei != spanningTree.end(); ++ei)
    {
        const edge_t& edge = *ei;
        minGraph[edge.m_source].edges.push_back(edge.m_target);
        minGraph[edge.m_target].edges.push_back(edge.m_source);
    }
    const size_t nEdges = spanningTree.size();
    return nEdges;
#else

    // A-- Compute the number of node we need
    std::set<size_t> setNodes;
    for (size_t p = 0; p < RelRs.size(); ++p)
    {
        const RelativeRotation& relR = RelRs[p];
        setNodes.insert(relR.i);
        setNodes.insert(relR.j);
    }

    // B-- Create a node graph for each element of the set
    typedef std::map<size_t, graph_t::Node> map_Size_t_Node;
    map_Size_t_Node map_size_t_to_node;
    for (std::set<size_t>::const_iterator iter = setNodes.begin(); iter != setNodes.end(); ++iter)
    {
        map_size_t_to_node[*iter] = g.addNode();
    }

    // C-- Create a graph from RelRs with weighted edges
    map_EdgeMap map_edgeMap(g);
    for (size_t p = 0; p < RelRs.size(); ++p)
    {
        const RelativeRotation& relR = RelRs[p];
        mapIJ2R[std::make_pair(relR.i, relR.j)] = relR.Rij;
        mapIJ2R[std::make_pair(relR.j, relR.i)] = relR.Rij.transpose();

        // add edge to the graph
        graph_t::Edge edge = g.addEdge(map_size_t_to_node[relR.i], map_size_t_to_node[relR.j]);
        map_edgeMap[edge] = -relR.weight;
    }

    // D-- Compute the MST of the graph
    std::vector<graph_t::Edge> tree_edge_vec;
    lemon::kruskal(g, map_edgeMap, std::back_inserter(tree_edge_vec));

    const size_t nViews = lemon::countNodes(g);
    minGraph.resize(nViews);

    // E-- Export compute MST
    for (size_t i = 0; i < tree_edge_vec.size(); i++)
    {
        minGraph[g.id(g.u(tree_edge_vec[i]))].edges.push_back(g.id(g.v(tree_edge_vec[i])));
        minGraph[g.id(g.v(tree_edge_vec[i]))].edges.push_back(g.id(g.u(tree_edge_vec[i])));
    }
    return tree_edge_vec.size();
#endif
}
//----------------------------------------------------------------

// Filter the given relative rotations using the known global rotations
// returns the number of inliers
unsigned int FilterRelativeRotations(const RelativeRotations& RelRs, const Matrix3x3Arr& Rs, float threshold, std::vector<bool>* vec_inliers)
{
    assert(!RelRs.empty() && !Rs.empty());
    assert(threshold >= 0);
    // compute errors for each relative rotation
    std::vector<float> errors(RelRs.size());
    for (int r = 0; r < RelRs.size(); ++r)
    {
        const RelativeRotation& relR = RelRs[r];
        const Matrix3x3& Ri = Rs[relR.i];
        const Matrix3x3& Rj = Rs[relR.j];
        const Matrix3x3& Rij = relR.Rij;
        const Mat3 eRij(Rj.transpose() * Rij * Ri);
        const aliceVision::Vec3 erij;
        ceres::RotationMatrixToAngleAxis((const double*)eRij.data(), (double*)erij.data());
        errors[r] = (float)erij.norm();
    }
    if (threshold == 0)
    {
        // estimate threshold
        const std::pair<float, float> res = ComputeX84Threshold(&errors[0], errors.size());
        threshold = res.first + res.second;
    }
    if (vec_inliers)
    {
        vec_inliers->resize(RelRs.size());
    }
    // mark outliers
    unsigned int nInliers = 0;
    for (int r = 0; r < errors.size(); ++r)
    {
        bool bInlier = (errors[r] < threshold);
        if (vec_inliers)
            (*vec_inliers)[r] = bInlier;
        if (bInlier)
            ++nInliers;
    }
    return nInliers;
}  // FilterRelativeRotations
//----------------------------------------------------------------

REAL RelRotationAvgError(const RelativeRotations& RelRs, const Matrix3x3Arr& Rs, REAL* pMin = nullptr, REAL* pMax = nullptr)
{
#ifdef ALICEVISION_ROTATION_AVERAGING_WITH_BOOST
    boost::accumulators::
      accumulator_set<REAL, boost::accumulators::stats<boost::accumulators::tag::min, boost::accumulators::tag::mean, boost::accumulators::tag::max>>
        acc;

    for (int i = 0; i < RelRs.size(); ++i)
    {
        const RelativeRotation& relR = RelRs[i];
        acc(aliceVision::FrobeniusNorm(relR.Rij - (Rs[relR.j] * Rs[relR.i].transpose())));
    }
    if (pMin)
        *pMin = boost::accumulators::min(acc);
    if (pMax)
        *pMax = boost::accumulators::max(acc);
    return boost::accumulators::mean(acc);
#else
    std::vector<REAL> vec_err(RelRs.size(), REAL(0.0));
    for (int i = 0; i < RelRs.size(); ++i)
    {
        const RelativeRotation& relR = RelRs[i];
        vec_err[i] = aliceVision::FrobeniusNorm(relR.Rij - (Rs[relR.j] * Rs[relR.i].transpose()));
    }
    BoxStats<float> stats(vec_err.begin(), vec_err.end());
    if (pMin)
        *pMin = stats.min;
    if (pMax)
        *pMax = stats.max;
    return stats.mean;
#endif
}
//----------------------------------------------------------------

void InitRotationsMST(const RelativeRotations& RelRs, Matrix3x3Arr& Rs, const size_t nMainViewID)
{
    assert(!Rs.empty());

    // -- Compute coarse global rotation estimates:
    //   - by finding the maximum spanning tree and linking the relative rotations
    //   - Initial solution is driven by relative rotations data confidence.
    graph_t g;
    MapEdgeIJ2R mapIJ2R;
    NodeArr minGraph;
    // find the Maximum Spanning Tree
    FindMaximumSpanningTree(RelRs, g, mapIJ2R, minGraph);
    g.clear();

    // start from the main view and link all views using the relative rotation estimates
    LinkQue stack;
    stack.push(Link(nMainViewID, size_t(0)));
    Rs[nMainViewID] = Matrix3x3::Identity();
    do
    {
        const Link& link = stack.front();
        const Node& node = minGraph[link.ID];

        for (Node::InternalType::const_iterator pEdge = node.edges.begin(); pEdge != node.edges.end(); ++pEdge)
        {
            const size_t edge = *pEdge;
            if (edge == link.parentID)
            {
                // compute the global rotation for the current node
                assert(mapIJ2R.find(std::make_pair(link.parentID, link.ID)) != mapIJ2R.end());
                const Matrix3x3& Rij = mapIJ2R[std::make_pair(link.parentID, link.ID)];
                Rs[link.ID] = Rij * Rs[link.parentID];
            }
            else
            {
                // add edge to the processing queue
                stack.push(Link(edge, link.ID));
            }
        }
        stack.pop();
    } while (!stack.empty());
}

// Robustly estimate global rotations from relative rotations as in:
// "Efficient and Robust Large-Scale Rotation Averaging", Chatterjee and Govindu, 2013
// and detect outliers relative rotations and return them with 0 in arrInliers
bool GlobalRotationsRobust(const RelativeRotations& RelRs,
                           Matrix3x3Arr& Rs,
                           const size_t nMainViewID,
                           float threshold,
                           std::vector<bool>* vec_Inliers)
{
    assert(!Rs.empty());

    // -- Compute coarse global rotation estimates:
    InitRotationsMST(RelRs, Rs, nMainViewID);

    // refine global rotations based on the relative rotations
    const bool bOk = RefineRotationsAvgL1IRLS(RelRs, Rs, nMainViewID);

    // find outlier relative rotations
    if (threshold >= 0 && vec_Inliers)
    {
        FilterRelativeRotations(RelRs, Rs, threshold, vec_Inliers);
    }

    return bOk;
}  // GlobalRotationsRobust
//----------------------------------------------------------------

// build A in Ax=b
inline void _FillMappingMatrix(const RelativeRotations& RelRs, const size_t nMainViewID, Eigen::SparseMatrix<REAL, Eigen::ColMajor>& A)
{
    A.reserve(A.rows() * 2);  // estimate of the number of non-zeros (optional)
    Eigen::SparseMatrix<REAL, Eigen::ColMajor>::Index i = 0, j = 0;
    for (int r = 0; r < RelRs.size(); ++r)
    {
        const RelativeRotation& relR = RelRs[r];
        if (relR.i != nMainViewID)
        {
            j = 3 * (relR.i < nMainViewID ? relR.i : relR.i - 1);
            A.insert(i + 0, j + 0) = REAL(-1);
            A.insert(i + 1, j + 1) = REAL(-1);
            A.insert(i + 2, j + 2) = REAL(-1);
        }
        if (relR.j != nMainViewID)
        {
            j = 3 * (relR.j < nMainViewID ? relR.j : relR.j - 1);
            A.insert(i + 0, j + 0) = REAL(1);
            A.insert(i + 1, j + 1) = REAL(1);
            A.insert(i + 2, j + 2) = REAL(1);
        }
        i += 3;
    }
    A.makeCompressed();
}

// compute errors for each relative rotation
inline void _FillErrorMatrix(const RelativeRotations& RelRs, const Matrix3x3Arr& Rs, Eigen::Matrix<REAL, Eigen::Dynamic, 1>& b)
{
    for (size_t r = 0; r < RelRs.size(); ++r)
    {
        const RelativeRotation& relR = RelRs[r];
        const Matrix3x3& Ri = Rs[relR.i];
        const Matrix3x3& Rj = Rs[relR.j];
        const Matrix3x3& Rij = relR.Rij;
        const Mat3 eRij(Rj.transpose() * Rij * Ri);
        const aliceVision::Vec3 erij;
        ceres::RotationMatrixToAngleAxis((const double*)eRij.data(), (double*)erij.data());
        b.block<3, 1>(3 * r, 0) = aliceVision::Vec3(erij * relR.weight);
    }
}

// apply correction to global rotations
inline void _CorrectMatrix(const Eigen::Matrix<REAL, Eigen::Dynamic, 1>& x, const size_t nMainViewID, Matrix3x3Arr& Rs)
{
    for (size_t r = 0; r < Rs.size(); ++r)
    {
        if (r == nMainViewID)
            continue;
        Matrix3x3& Ri = Rs[r];
        const size_t i = (r < nMainViewID ? r : r - 1);
        aliceVision::Vec3 eRid = aliceVision::Vec3(x.block<3, 1>(3 * i, 0));
        const Mat3 eRi;
        ceres::AngleAxisToRotationMatrix((const double*)eRid.data(), (double*)eRi.data());
        Ri = Ri * eRi;
    }
}

// Refine the global rotations using to the given relative rotations, similar to:
// "Efficient and Robust Large-Scale Rotation Averaging", Chatterjee and Govindu, 2013
// L1 Rotation Averaging (L1RA) and Iteratively Reweighted Least Squares (IRLS) implementations combined
bool RefineRotationsAvgL1IRLS(const RelativeRotations& RelRs, Matrix3x3Arr& Rs, const size_t nMainViewID, REAL sigma)
{
    assert(!RelRs.empty() && !Rs.empty());
    assert(Rs[nMainViewID] == Matrix3x3::Identity());

    REAL fMinBefore, fMaxBefore, fMeanBefore = RelRotationAvgError(RelRs, Rs, &fMinBefore, &fMaxBefore);

    const unsigned nObss = (unsigned)RelRs.size();
    const unsigned nVars = (unsigned)Rs.size() - 1;  // main view is kept constant
    const unsigned m = nObss * 3;
    const unsigned n = nVars * 3;

    // build mapping matrix A in Ax=b
    Eigen::SparseMatrix<REAL, Eigen::ColMajor> A(m, n);
    _FillMappingMatrix(RelRs, nMainViewID, A);

    // init x with 0 that corresponds to trusting completely the initial Ri guess
    Vec x(Vec::Zero(n)), b(m);

    // L1RA iterate optimization till the desired precision is reached
    REAL e = std::numeric_limits<REAL>::max(), ep;
    unsigned iter1 = 0;
    do
    {
        // compute errors for each relative rotation
        _FillErrorMatrix(RelRs, Rs, b);
        // solve the linear system using l1 norm
        if (!RobustRegressionL1PD(A, b, x))
        {
            ALICEVISION_LOG_WARNING("error: l1 robust regression failed.");
            return false;
        }
        ep = e;
        e = x.norm();
        if (ep < e)
            break;
        // apply correction to global rotations
        _CorrectMatrix(x, nMainViewID, Rs);
    } while (++iter1 < 32 && e > 1e-5 && (ep - e) / e > 1e-2);
    // IRLS iterate optimization till the desired precision is reached
    x.setZero();
    e = std::numeric_limits<REAL>::max();
    unsigned iter2 = 0;
    do
    {
        // compute errors for each relative rotation
        _FillErrorMatrix(RelRs, Rs, b);
        // solve the linear system using l2 norm
        if (!IterativelyReweightedLeastSquares(A, b, x, sigma))
        {
            ALICEVISION_LOG_WARNING("error: l2 iterative regression failed");
            return false;
        }
        ep = e;
        e = x.norm();
        if (ep < e)
            break;
        // apply correction to global rotations
        _CorrectMatrix(x, nMainViewID, Rs);
    } while (++iter2 < 32 && e > 1e-5 && (ep - e) / e > 1e-2);

    REAL fMinAfter, fMaxAfter, fMeanAfter = RelRotationAvgError(RelRs, Rs, &fMinAfter, &fMaxAfter);

    ALICEVISION_LOG_DEBUG("Refine global rotations using L1RA-IRLS and "
                          << nObss << " relative rotations:\n"
                          << " error reduced from " << fMeanBefore << "(" << fMinBefore << " min, " << fMaxBefore << " max)\n"
                          << " to " << fMeanAfter << "(" << fMinAfter << "min," << fMaxAfter << "max)\n"
                          << " in " << iter1 << "+" << iter2 << "=" << iter1 + iter2 << " iterations");

    return true;
}  // RefineRotationsAvgL1IRLS

}  // namespace l1
}  // namespace rotationAveraging
}  // namespace aliceVision
